#include "stm32f10x.h"
#include "math.h"
#include "maths.h"
#include "struct_all.h"
#include "MPU6050.h"
#include "filter.h"
#include "imu.h"

struct _angle angle;

void Prepare_Data()
{
    MPU6050_Read();    //触发读取 ，立即返回
	MPU6050_Offset();  //对MPU6050进行处理，减去零偏。如果没有计算零偏就计算零偏

    
}

// ==================================================================================
// 描述:
// 必须定义'halfT '为周期的一半，以及滤波器的参数Kp和Ki
// 四元数'q0', 'q1', 'q2', 'q3'定义为全局变量
// 需要在每一个采样周期调用'IMUupdate()'函数
// 陀螺仪数据单位是弧度/秒，加速度计的单位无关重要，因为会被规范化
// 获取姿态角 求解四元数和欧拉角都在此函数中完成
// ==================================================================================
// kp,ki后面做测试
#define Kp 1.50f      // 比例常数      // proportional gain governs rate of convergence to accelerometer/magnetometer
                      // 比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.001f     // 积分常数      // integral gain governs rate of convergence of gyroscope biases
                      // 积分增益控制陀螺偏差的收敛速度
#define halfT 0.0005f // 半周期
#define T     0.001f  // 周期为1ms
// ==================================================================================
// 变量定义
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;  // 四元数
float exInt = 0, eyInt = 0, ezInt = 0; // 误差积分累计值

// ==================================================================================
// 函数原型：void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
// 功        能：互补滤波进行姿态解算
// 输        入：陀螺仪数据及加速度计数据
// ==================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    float norm;
    float vx, vy, vz; // 实际重力加速度
    float ex, ey, ez; // 叉积计算的误差

    float hx, hy, hz, bx, bz;
    float wx, wy, wz;
    // float matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };//初始化矩阵

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    // 四元数积分，求得当前的姿态
    float q0_last = q0;
    float q1_last = q1;
    float q2_last = q2;
    float q3_last = q3;

    norm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= norm;
    my *= norm;
    mz *= norm;

    // 将b系中的地磁计分量[mx,my,mz]转换到n系,得到[hx,hy,hz]
    hx = 2 * (mx * (0.5 - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2 * (mx * (q1q2 + q0q3) + my * (0.5 - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
    // 得到n系中的地磁向量的真实值[bx,bz,by],其中by=0
    bx = sqrt(hx * hx + hy * hy);
    bz = hz;

    // Estimated direction of magnetic field
    // n系中的地磁向量[bx，by,bz]转换到b系中，得到[wx,wy,wz]
    wx = bx * (0.5 - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    wy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    wz = bx * (q0q2 + q1q3) + bz * (0.5 - q1q1 - q2q2);

    // Error is sum of cross product between estimated direction and measured direction of field vectors
    // ex += (my * halfwz - mz * halfwy);
    // ey += (mz * halfwx - mx * halfwz);
    // ez += (mx * halfwy - my * halfwx);

    // 把加速度计的三维向量转成单位向量,
    // 加速度计测量的重力方向(机体坐标系
    norm = invSqrt(ax * ax + ay * ay + az * az);
    ax   = ax * norm;
    ay   = ay * norm;
    az   = az * norm;

    // //矩阵R 将惯性坐标系(n)转换到机体坐标系(b)
    // matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;	// 11(前列后行)
    // matrix[1] = 2.f * (q1q2 + q0q3);	    // 12
    // matrix[2] = 2.f * (q1q3 - q0q2);	    // 13
    // matrix[3] = 2.f * (q1q2 - q0q3);	    // 21
    // matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;	// 22
    // matrix[5] = 2.f * (q2q3 + q0q1);	    // 23
    // matrix[6] = 2.f * (q1q3 + q0q2);	    // 31
    // matrix[7] = 2.f * (q2q3 - q0q1);	    // 32
    // matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;	// 33

    // 估计重力加速度方向在飞行器坐标系中的表示，为四元数表示的旋转矩阵的第三行
    // 四元数推出的实际重力方向(机体坐标系)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    // 加速度计读取的方向与重力加速度方向的差值，用向量叉乘计算 叉积误差
    ex = (ay * vz - az * vy) + (my * wz - mz * wy);
    ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
    ez = (ax * vy - ay * vx) + (mx * wy - my * wx);

    // 误差累积，已与积分常数相乘 ,叉积误差积分为角速度
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // 用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量,角速度补偿
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    // 一阶近似算法 更新四元数
    q0 = q0_last + (-q1_last * gx - q2_last * gy - q3_last * gz) * halfT;
    q1 = q1_last + (q0_last * gx + q2_last * gz - q3_last * gy) * halfT;
    q2 = q2_last + (q0_last * gy - q1_last * gz + q3_last * gx) * halfT;
    q3 = q3_last + (q0_last * gz + q1_last * gy - q2_last * gx) * halfT;

    //	//二阶近似算法
    //	float delta2 = (gx*gx + gy*gy + gz*gz)*T*T;
    //	q0 = q0_last*(1-delta2/8) + (-q1_last*gx - q2_last*gy - q3_last*gz)*halfT;
    //	q1 = q1_last*(1-delta2/8) + ( q0_last*gx + q2_last*gz - q3_last*gy)*halfT;
    //	q2 = q2_last*(1-delta2/8) + ( q0_last*gy - q1_last*gz + q3_last*gx)*halfT;
    //	q3 = q3_last*(1-delta2/8) + ( q0_last*gz + q1_last*gy - q2_last*gx)*halfT;

    //	//三阶近似算法
    //	float delta2 = (gx*gx + gy*gy + gz*gz)*T*T;
    //	q0 = q0_last*(1-delta2/8) + (-q1_last*gx - q2_last*gy - q3_last*gz)*T*(0.5 - delta2/48);
    //	q1 = q1_last*(1-delta2/8) + ( q0_last*gx + q2_last*gz - q3_last*gy)*T*(0.5 - delta2/48);
    //	q2 = q2_last*(1-delta2/8) + ( q0_last*gy - q1_last*gz + q3_last*gx)*T*(0.5 - delta2/48);
    //	q3 = q3_last*(1-delta2/8) + ( q0_last*gz + q1_last*gy - q2_last*gx)*T*(0.5 - delta2/48);

    //	//四阶近似算法
    //	float delta2 = (gx*gx + gy*gy + gz*gz)*T*T;
    //	q0 = q0_last*(1 - delta2/8 + delta2*delta2/384) + (-q1_last*gx - q2_last*gy - q3_last*gz)*T*(0.5 - delta2/48);
    //	q1 = q1_last*(1 - delta2/8 + delta2*delta2/384) + ( q0_last*gx + q2_last*gz - q3_last*gy)*T*(0.5 - delta2/48);
    //	q2 = q2_last*(1 - delta2/8 + delta2*delta2/384) + ( q0_last*gy - q1_last*gz + q3_last*gx)*T*(0.5 - delta2/48);
    //	q3 = q3_last*(1 - delta2/8 + delta2*delta2/384) + ( q0_last*gz + q1_last*gy - q2_last*gx)*T*(0.5 - delta2/48);

    // 单位化四元数
    norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0   = q0 * norm;
    q1   = q1 * norm;
    q2   = q2 * norm;
    q3   = q3 * norm;

    angle.pitch = -asin(2 * (q1*q3 - q0*q2))* 57.3f;
    angle.roll = atan2(2 * q2*q3 + 2.f * q0*q1, q0*q0 - q1*q1 - q2*q2 + q3*q3)* 57.3f;
    angle.yaw = atan2(2 * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3)* 57.3f;

    // 	//四元数反解欧拉角
    // Yaw = atan2(2.f * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3)* 57.3f;
    // Pitch = -asin(2.f * (q1*q3 - q0*q2))* 57.3f;
    // Roll = atan2(2.f * q2*q3 + 2.f * q0*q1, q0*q0 - q1*q1 - q2*q2 + q3*q3)* 57.3f;

    // 	out_angle.yaw  +=  filter_gyro.z * RawData_to_Angle * 0.001f;
}



